//==========================================================================
// Copyright (c) 2000-2008,  Elastos, Inc.  All Rights Reserved.
//==========================================================================
#include <ddk.h>
#include "pm.h"
#include <serialio.h>
#include <bulverde.h>

int pm_do_idle();
void DumpGPIORegs();
static int get_cpu_speed();
ECode CPower::Control(
        /* [in] */ INT nControlCode,
        /* [in] */ EzByteBuf ebbInData,
        /* [out] */ EzByteBuf * pOutData,
        /* [out] */ IEvent * * ppCompletionEvent)
{
    ECode ec = NOERROR;
    switch(nControlCode)
    {
        //just for debug
        case 5:
            break;
        case 6:
            break;
        case 100:
        {
            break;
        }

        case 101:
        {
        }

        case 102:
        {
            DumpGPIORegs();
            break;
        }
        case 103:
        {
            //pcf50606_dump_registers();
            break;
        }
        case 104:
        {
            kprintf("now run at %d\n",get_cpu_speed());
            break;
        }

        //for power manager call
        case 1000:
            break;
        case 1003:
            pm_do_idle();
            break;
        case 1005:
            break;

        default:
            ec = E_NOT_IMPLEMENTED;
            break;
    }
    return ec;
}

ECode CPower::Read(
        /* [in] */ UINT64 u64Offset,
        /* [in] */ UINT uNumberOfBytesToRead,
        /* [out] */ EzByteBuf ebbData,
        /* [out] */ IEvent * * ppCompletionEvent)
{
    return E_NOT_IMPLEMENTED;
}

ECode  CPower::Write(
        /* [in] */ UINT64 u64Offset,
        /* [in] */ EzByteBuf ebbData,
        /* [out] */ UINT * puNumberOfBytesWritten,
        /* [out] */ IEvent * * ppCompletionEvent)
{
    return E_NOT_IMPLEMENTED;
}

int Gpio_GetDir(int gpio_num)
{
    int GPDR=0;
    int dir=0x1;
    if (gpio_num < 32)
    {
        GPDR = GPDR0;
    }
    else if (gpio_num < 64)
    {
        GPDR = GPDR1;
    }
    else if (gpio_num < 96)
    {
        GPDR = GPDR2;
    }
    else if (gpio_num < 121)
    {
        GPDR = GPDR3;
    }
    dir &= GPDR >> (gpio_num % 32);
    return dir;
}

int  Gpio_GetLevel(int gpio_num)
{
    int Level=0;
    int GPLR=0;
    if (gpio_num < 32)
    {
        GPLR = GPLR0;
    }
    else if (gpio_num < 64)
    {
        GPLR = GPLR1;
    }
    else if (gpio_num < 96)
    {
        GPLR = GPLR2;
    }
    else if (gpio_num < 121)
    {
        GPLR = GPLR3;
    }
    Level = GPLR >> (gpio_num % 32);
    return Level&0x1;
}

int  Gpio_GetAFR(int gpio_num)
{
    int function =0;
    int GAFR=0;
    if (gpio_num < 16)
    {
        GAFR = GAFR0_L;
    }
    else if (gpio_num < 32)
    {
        GAFR = GAFR0_U;
    }
    else if (gpio_num < 48)
    {
        GAFR = GAFR1_L;
    }
    else if (gpio_num < 64)
    {
        GAFR = GAFR1_U;
    }
    else if (gpio_num < 80)
    {
        GAFR = GAFR2_L;
    }
    else if (gpio_num < 96)
    {
        GAFR = GAFR2_U;
    }
    else if (gpio_num < 112)
    {
        GAFR = GAFR3_L;
    }
    else if (gpio_num < 121)
    {
        GAFR = GAFR3_U;
    }
    function = GAFR>>((gpio_num % 16) * 2);
    return function&0x3;
}

void DumpGPIORegs()
{
    kprintf("Dump GPIO register direct information\n");
    kprintf("GPLR0    %12x           ",GPLR0);
    kprintf("GPLR1    %12x           \n",GPLR1);
    kprintf("GPLR2    %12x           ",GPLR2);
    kprintf("GPLR3    %12x           \n",GPLR3);
    kprintf("GPDR0    %12x           ",GPDR0);
    kprintf("GPDR1    %12x           \n",GPDR1);
    kprintf("GPDR2    %12x           ",GPDR2);
    kprintf("GPDR3    %12x           \n",GPDR3);
    kprintf("GRER0    %12x           ",GRER0);
    kprintf("GRER1    %12x           \n",GRER1);
    kprintf("GRER2    %12x           ",GRER2);
    kprintf("GRER3    %12x           \n",GRER3);
    kprintf("GFER0    %12x           ",GFER0);
    kprintf("GFER1    %12x           \n",GFER1);
    kprintf("GFER2    %12x           ",GFER2);
    kprintf("GFER3    %12x           \n",GFER3);
    kprintf("GEDR0    %12x           ",GEDR0);
    kprintf("GEDR1    %12x           \n",GEDR1);
    kprintf("GEDR2    %12x           ",GEDR2);
    kprintf("GEDR3    %12x           \n",GEDR3);
    kprintf("GAFR0_L    %12x           ",GAFR0_L);
    kprintf("GAFR0_U    %12x           \n",GAFR0_U);
    kprintf("GAFR1_L    %12x           ",GAFR1_L);
    kprintf("GAFR1_U    %12x           \n",GAFR1_U);
    kprintf("GAFR2_L    %12x           ",GAFR2_L);
    kprintf("GAFR2_U    %12x           \n",GAFR2_U);
    kprintf("GAFR3_L    %12x           ",GAFR3_L);
    kprintf("GAFR3_U    %12x           \n",GAFR3_U);
    kprintf("MDREFR    %12x           \n",MDREFR);
    kprintf("OSCC    %12x           \n",OSCC);
    kprintf("Dump GPIO register more readable information\n");
    kprintf(" GPIO PIN NAME    DIRECT    LEVEL    ALT FUNC\n");
    for(int i=0;i<121;i++)
    {
        kprintf(" GPIO %5d%10d%9d%9d\n",i,Gpio_GetDir(i),Gpio_GetLevel(i),Gpio_GetAFR(i));
    }
}

void DumpPCRegs()
{
//dump power manager and clock manager registers.
    kprintf("PMCR    %10x           ",__REG(0x40f00000));
    kprintf("PSSR    %10x           ",__REG(0x40f00004));
    kprintf("PSPR    %10x           \n",__REG(0x40f00008));
    kprintf("PWER    %10x           ",__REG(0x40f0000c));
    kprintf("PRER    %10x           ",__REG(0x40f00010));
    kprintf("PFER    %10x           \n",__REG(0x40f00014));
    kprintf("PEDR    %10x           ",__REG(0x40f00018));
    kprintf("PCFR    %10x           ",__REG(0x40f0001c));
    kprintf("PGSR0   %10x           \n",__REG(0x40f00020));
    kprintf("PGSR1   %10x           ",__REG(0x40f00024));
    kprintf("PGSR2   %10x           ",__REG(0x40f00028));
    kprintf("PGSR3   %10x           \n",__REG(0x40f0002c));
    kprintf("RCSR    %10x           ",__REG(0x40f00030));
    kprintf("PSLR    %10x           ",__REG(0x40f00034));
    kprintf("PSTR    %10x           \n",__REG(0x40f00038));
    kprintf("PVCR    %10x           ",__REG(0x40f00040));
    kprintf("PUCR    %10x           ",__REG(0x40f0004c));
    kprintf("PKWR    %10x           \n",__REG(0x40f00050));
    kprintf("PKSR    %10x           ",__REG(0x40f00054));
    kprintf("CCCR    %10x           ",__REG(0x41300000));
    kprintf("CKEN    %10x           \n",__REG(0x41300004));
    kprintf("OSCC    %10x           ",__REG(0x41300008));
    kprintf("CCSR    %10x           \n",__REG(0x4130000c));
    kprintf("OSCR0   %10x           \n",__REG(0x40a00010));
}

static CPower g_Power;
unsigned int sleep_phys_sp;
#define OSTR_BASE   0x40A00000
#define REG(x)    (*(volatile unsigned long*)(((x)+ 0x40000000)))
#define OMCR4 REG(OSTR_BASE+OMCR4_OFFSET)
#define OMCR5     REG(OSTR_BASE+OMCR5_OFFSET)
#define OMCR6     REG(OSTR_BASE+OMCR6_OFFSET)
#define OMCR7     REG(OSTR_BASE+OMCR7_OFFSET)

#define OSMR4     REG(OSTR_BASE+OSMR4_OFFSET)
#define OSMR5     REG(OSTR_BASE+OSMR5_OFFSET)
#define OSMR6     REG(OSTR_BASE+OSMR6_OFFSET)
#define OSMR7     REG(OSTR_BASE+OSMR7_OFFSET)

enum {  SLEEP_SAVE_START = 0,

    SLEEP_SAVE_OSCR0, SLEEP_SAVE_OIER,
    SLEEP_SAVE_OSMR0, SLEEP_SAVE_OSMR1, SLEEP_SAVE_OSMR2, SLEEP_SAVE_OSMR3,
    SLEEP_SAVE_OSMR4, SLEEP_SAVE_OSMR5, SLEEP_SAVE_OSMR6, SLEEP_SAVE_OSMR7,
    SLEEP_SAVE_OMCR4, SLEEP_SAVE_OMCR5, SLEEP_SAVE_OMCR6, SLEEP_SAVE_OMCR7,
    SLEEP_SAVE_GPDR0, SLEEP_SAVE_GPDR1, SLEEP_SAVE_GPDR2,
    SLEEP_SAVE_GRER0, SLEEP_SAVE_GRER1, SLEEP_SAVE_GRER2,
    SLEEP_SAVE_GFER0, SLEEP_SAVE_GFER1, SLEEP_SAVE_GFER2,
    SLEEP_SAVE_GAFR0_L, SLEEP_SAVE_GAFR1_L, SLEEP_SAVE_GAFR2_L,
    SLEEP_SAVE_GAFR0_U, SLEEP_SAVE_GAFR1_U, SLEEP_SAVE_GAFR2_U,
    SLEEP_SAVE_GPDR3, SLEEP_SAVE_GRER3, SLEEP_SAVE_GFER3,
    SLEEP_SAVE_GAFR3_L, SLEEP_SAVE_GAFR3_U,
    SLEEP_SAVE_ICMR,
    SLEEP_SAVE_CKEN,
    SLEEP_SAVE_PWER,
    SLEEP_SAVE_PSTR,
    SLEEP_SAVE_PCFR,
    SLEEP_SAVE_PRER,
    SLEEP_SAVE_PFER,
    SLEEP_SAVE_PKWR,
    SLEEP_SAVE_OSCR4,
    SLEEP_SAVE_MDREFR,
    SLEEP_SAVE_OSSR,
    SLEEP_SAVE_PGSR0,
    SLEEP_SAVE_PGSR1,
    SLEEP_SAVE_PGSR2,
    SLEEP_SAVE_PGSR3,
    SLEEP_SAVE_CKSUM11,
    SLEEP_SAVE_SIZE11,
};

unsigned long sleep_save[SLEEP_SAVE_SIZE11];
void saveregister(void)
{
    /*For SDRAM sightings.  */
    SAVE(MDREFR);
    /* save vital registers */
    SAVE(OSCR0);
    SAVE(OSSR);
    SAVE(OSMR0);
    SAVE(OSMR1);
    SAVE(OSMR2);
    SAVE(OSMR3);
    SAVE(OSMR4);
    SAVE(OSMR5);
    SAVE(OSMR6);
    SAVE(OSMR7);
    SAVE(OMCR4);
    SAVE(OMCR5);
    SAVE(OMCR6);
    SAVE(OMCR7);
    SAVE(OIER);

    SAVE(GPDR0); SAVE(GPDR1); SAVE(GPDR2);
    SAVE(GRER0); SAVE(GRER1); SAVE(GRER2);
    SAVE(GFER0); SAVE(GFER1); SAVE(GFER2);
    SAVE(GAFR0_L); SAVE(GAFR0_U);
    SAVE(GAFR1_L); SAVE(GAFR1_U);
    SAVE(GAFR2_L); SAVE(GAFR2_U);

    SAVE(GPDR3);
    SAVE(GRER3);
    SAVE(GFER3);
    SAVE(GAFR3_L); SAVE(GAFR3_U);

    SAVE(ICMR);
    ICMR = 0;

    SAVE(CKEN);

    SAVE(PWER);
    SAVE(PCFR);
    SAVE(PRER);
    SAVE(PFER);
    SAVE(PKWR);
    SAVE(PSTR);

    SAVE(PGSR0);
    SAVE(PGSR1);
    SAVE(PGSR2);
    SAVE(PGSR3);
}

void restoreregister(void)
{
    RESTORE(PGSR0);
    RESTORE(PGSR1);
    RESTORE(PGSR2);
    RESTORE(PGSR3);
    RESTORE(GPDR0); RESTORE(GPDR1); RESTORE(GPDR2);
    RESTORE(GRER0); RESTORE(GRER1); RESTORE(GRER2);
    RESTORE(GFER0); RESTORE(GFER1); RESTORE(GFER2);
    RESTORE(GAFR0_L); RESTORE(GAFR0_U);
    RESTORE(GAFR1_L); RESTORE(GAFR1_U);
    RESTORE(GAFR2_L); RESTORE(GAFR2_U);
    RESTORE(GAFR3_L); RESTORE(GAFR3_U);
    RESTORE(GPDR3);
    RESTORE(GRER3);
    RESTORE(GFER3);
    PSSR = PSSR_PH|PSSR_OTGPH|PSSR_RDH;
    (void *) PSSR;
    RESTORE(CKEN);
    RESTORE(OSMR0);
    RESTORE(OSMR1);
    RESTORE(OSMR2);
    RESTORE(OSMR3);
    RESTORE(OSMR4);
    RESTORE(OSMR5);
    RESTORE(OSMR6);
    RESTORE(OSMR7);
    RESTORE(OMCR4);
    RESTORE(OMCR5);
    RESTORE(OMCR6);
    RESTORE(OMCR7);
    RESTORE(OSCR0);
    RESTORE(OSSR);
    RESTORE(OIER);

    RESTORE(MDREFR);


    RESTORE(PWER); RESTORE(PCFR); RESTORE(PRER);
	RESTORE(PFER); RESTORE(PKWR); RESTORE(PSTR);
    ICLR = 0;
    ICCR = 1;
    RESTORE(ICMR);
}

EXTERN_C unsigned long __cdecl get_sleep_phys_sp(void *sp)
{
    kprintf("sleep sp %x -> %x, OSCR %08x >>>>>>>>\n", sp, sleep_phys_sp, OSCR0);
    return sleep_phys_sp;
}

int pm_do_idle()
{
    //0x1,means cpu enter idle mode
    WritePWRMODE(0x1);
    return 0;
}

static int get_cpu_speed()
{
    int speed, N2, L;
    unsigned long cccr = CCCR;
    int mode = get_cpu_mode();

    N2 = (cccr & CCCR_2N_MASK) >> 7/*CCCR_2N_SHIFT*/;
    L = (cccr & CCCR_L_MASK);
    if(N2 < 2)
        N2 = 2;
    if(L < 2)
        L = 2;

    speed = BASE_FREQ * L;
    if(mode & CLKCFG_HT)
        speed = (speed * N2) / 4;
    else if(mode & CLKCFG_T)
        speed = (speed * N2) / 2;

    return speed;
}

EXTERN IDeviceDriver * CDECL CreatePower(uint_t uDeviceNo, void *pvParameter)
{
    //GPIO_Reset();
    kprintf("Add Power OK,now run at %d!\n",get_cpu_speed());
    return &g_Power;
}
